/*
 * File:   PrimaryStateTurn180.c
 * Author: craigjb
 *
 * Created on April 18, 2013, 3:57 PM
 */

#include "board.h"
#include "states.h"
#include "sensors.h"
#include "pwm.h"
#include "tick.h"

#define TURN_STATE_START 1
#define TURN_STATE_WAIT 2
#define TURN_STATE_FIND_TAPE 3
#define TURN_STATE_FOUND_TAPE 4

int currentTurnState = TURN_STATE_START;

#define TURN_LOOK_RIGHT SENSOR_RIGHT_PHOTO
#define TURN_LOOK_MIDDLE SENSOR_MIDDLE_PHOTO
int lookingFor = TURN_LOOK_RIGHT;

unsigned long int lastTurnUpdate = 0;

void TurnStateStart();
void TurnStateWait();
void TurnStateFindTape();
void TurnStateFoundTape();

void PrimaryStateTurn180(void)
{
    switch (currentTurnState)
    {
        case TURN_STATE_START:
            TurnStateStart();
            break;
        case TURN_STATE_WAIT:
            TurnStateWait();
            break;
        case TURN_STATE_FIND_TAPE:
            TurnStateFindTape();
            break;
        case TURN_STATE_FOUND_TAPE:
            TurnStateFoundTape();
            break;
        default:
            currentTurnState = TURN_STATE_START;
    }
}

// pivot at a medium speed
static const unsigned int kPivotSpeed = 1900;

void TurnStateStart()
{
    OC1R = kPivotSpeed;
    PivotCW();
    currentTurnState = TURN_STATE_WAIT;
    lookingFor = TURN_LOOK_RIGHT;
    lastTurnUpdate = GetTick();
}

/*const unsigned long int kWaitTime = 450;*/
const unsigned long int kWaitTime = 1000;

void TurnStateWait()
{
    if (GetTick() < lastTurnUpdate + kWaitTime)
        return;
    lastTurnUpdate = GetTick();
    currentTurnState = TURN_STATE_FIND_TAPE;
}

static const unsigned long int kTurnUpdateDelay = 5;

void TurnStateFindTape()
{
    /* Only update steering every 0.01 seconds*/
    if (GetTick() < lastTurnUpdate + kTurnUpdateDelay)
        return;
    lastTurnUpdate = GetTick();

    if (lookingFor == TURN_LOOK_RIGHT && IsRightPhotoOnTape() == 1)
            lookingFor = TURN_LOOK_MIDDLE;
    else if (lookingFor == TURN_LOOK_MIDDLE && IsMiddlePhotoOnTape() == 1)
    {
        currentTurnState = TURN_STATE_FOUND_TAPE;
        Stop();
    }
}

static const unsigned long int kTurnFoundTapeDelay = 2000;
static const unsigned int kFullSpeed = 1500;
static const unsigned int kSlowSpeed = 3500;

void TurnStateFoundTape()
{
    /* Only update steering every 0.05 seconds*/
    if (GetTick() < lastTurnUpdate + kTurnFoundTapeDelay)
        return;
    lastTurnUpdate = GetTick();

    currentTurnState = TURN_STATE_START;
    SetCurrentPrimaryState(PRIMARY_STATE_FOLLOW_TAPE);
    OC1R = kFullSpeed;
    OC2R = kFullSpeed;
    Forward();
}